#include <ros.h>

#include "diffbot_base_config.h"
#include "base_controller.h"
#include "adafruit_feather_wing/adafruit_feather_wing.h"

ros::NodeHandle nh;

using namespace diffbot;

AdafruitMotorController motor_controller_right = AdafruitMotorController(MOTOR_RIGHT);
AdafruitMotorController motor_controller_left = AdafruitMotorController(MOTOR_LEFT);

BaseController<AdafruitMotorController, Adafruit_MotorShield> base_controller(nh, &motor_controller_left, &motor_controller_right);

void setup()
{
    base_controller.setup();
    base_controller.init();

    nh.loginfo("初始化 DiffBot 电机控制器");
    motor_controller_left.begin();
    motor_controller_right.begin();
    nh.loginfo("设置完成");
}

void loop()
{
    // 基础控制器的主控制循环。
    // 该块根据定义的控制速率驱动机器人
    ros::Duration command_dt = nh.now() - base_controller.lastUpdateTime().control;
    if (command_dt.toSec() >= ros::Duration(1.0 / base_controller.publishRate().control_, 0).toSec())
    {
        base_controller.read();
        base_controller.write();
        base_controller.lastUpdateTime().control = nh.now();
    }

    // 当未收到来自高层 hardware_interface::RobotHW 的轮子命令时，该块停止电机
    command_dt = nh.now() - base_controller.lastUpdateTime().command_received;
    if (command_dt.toSec() >= ros::Duration(E_STOP_COMMAND_RECEIVED_DURATION, 0).toSec())
    {
        nh.logwarn("紧急停止");
        base_controller.eStop();
    }

    // 该块显示编码器读数。将 DEBUG 更改为 0 如果您不想显示
    if(base_controller.debug())
    {
        ros::Duration debug_dt = nh.now() - base_controller.lastUpdateTime().debug;
        if (debug_dt.toSec() >= base_controller.publishRate().period().debug_)
        {
            base_controller.printDebug();
            base_controller.lastUpdateTime().debug = nh.now();
        }
    }
    // 调用所有等待被调用的回调
    nh.spinOnce();
}